package Physics;

import Game.ObjectMaterial;

public class RigidBody {
	
	static float g_MotionBias = 0.5f;
	static float g_sleepEpsilon = 0.3f;
	
	// Member variables
	
	// mass
	float 	m_mass;
	float	m_inverseMass;
	
	// linear
	Vector3 m_position = new Vector3();
	Vector3	m_velocity = new Vector3();
	Vector3 m_acceleration = new Vector3();
	Vector3	m_lastFrameAcceleration = new Vector3();
	float	m_linearDamping;
	
	// angular
	Vector3		m_rotation = new Vector3();
	Quaternion	m_orientation = new Quaternion();
	float		m_angularDamping;
	
	// matrices
	Matrix4		m_transform = new Matrix4();
	Matrix3		m_inverseInertiaTensor = new Matrix3();
	Matrix3		m_inverseInertiaTensorWorld = new Matrix3();
	
	
	// newtonian
	Vector3		m_forceAccum = new Vector3();
	Vector3		m_torqueAccum = new Vector3();
	
	// states
	boolean m_isAwake;
	boolean m_canSleep;
	boolean	m_activated;
	float	m_motion;
	
	// hack for a rigid body to store its own material type
	// so it can pass it on to the health manager.
	ObjectMaterial m_material;
	
	
	// Member functions
	
	/**
	 * Constructor
	 */
	public RigidBody()
	{
		m_activated = true;
	}
	
	/**
	 * NOTE: No destructors in Java
	 */
	
	/**
	 * Calculates the matrix information of the 
	 * object, based on its current data
	 */
	public void calculateDerivedData()
	{
		m_orientation.normalize();
		
		// calculate the current transformation matrix
		calculateTransformMatrix();
		
		// calculate the current inertia tensor in world space
		transformInertiaTensor();
	}
	
	void calculateTransformMatrix()
	{
		m_transform.data[0] = 1 - 2*m_orientation.j*m_orientation.j - 2*m_orientation.k*m_orientation.k;
		m_transform.data[1] = 2*m_orientation.i*m_orientation.j - 2*m_orientation.r*m_orientation.k;
		m_transform.data[2] = 2*m_orientation.i*m_orientation.k + 2*m_orientation.r*m_orientation.j;
		m_transform.data[3] = m_position.x;
		
		m_transform.data[4] = 2*m_orientation.i*m_orientation.j + 2*m_orientation.r*m_orientation.k;
		m_transform.data[5] = 1 - 2*m_orientation.i*m_orientation.i - 2*m_orientation.k*m_orientation.k;
		m_transform.data[6] = 2*m_orientation.j*m_orientation.k - 2*m_orientation.r*m_orientation.i;
		m_transform.data[7] = m_position.y;
		
		m_transform.data[8] = 2*m_orientation.i*m_orientation.k - 2*m_orientation.r*m_orientation.j;
		m_transform.data[9] = 2*m_orientation.j*m_orientation.k + 2*m_orientation.r*m_orientation.i;
		m_transform.data[10] = 1 - 2*m_orientation.i*m_orientation.i - 2*m_orientation.j * m_orientation.j;
		m_transform.data[11] = m_position.z;
	}
	
	void transformInertiaTensor()
	{
		float t4 = m_transform.data[0] * 
				m_inverseInertiaTensor.data[0] + 
				m_transform.data[1] *
				m_inverseInertiaTensor.data[3] +
				m_transform.data[2] *
				m_inverseInertiaTensor.data[6];
		
		float t9 = m_transform.data[0] * 
				m_inverseInertiaTensor.data[1] + 
				m_transform.data[1] *
				m_inverseInertiaTensor.data[4] +
				m_transform.data[2] *
				m_inverseInertiaTensor.data[7];
		
		float t14 = m_transform.data[0] * 
				m_inverseInertiaTensor.data[2] + 
				m_transform.data[1] *
				m_inverseInertiaTensor.data[5] +
				m_transform.data[2] *
				m_inverseInertiaTensor.data[8];
		
		float t28 = m_transform.data[4] * 
				m_inverseInertiaTensor.data[0] + 
				m_transform.data[5] *
				m_inverseInertiaTensor.data[3] +
				m_transform.data[6] *
				m_inverseInertiaTensor.data[6];
		
		float t33 = m_transform.data[4] * 
				m_inverseInertiaTensor.data[1] + 
				m_transform.data[5] *
				m_inverseInertiaTensor.data[4] +
				m_transform.data[6] *
				m_inverseInertiaTensor.data[7];
		
		float t38 = m_transform.data[4] * 
				m_inverseInertiaTensor.data[2] + 
				m_transform.data[5] *
				m_inverseInertiaTensor.data[5] +
				m_transform.data[6] *
				m_inverseInertiaTensor.data[8];
		
		float t52 = m_transform.data[8] * 
				m_inverseInertiaTensor.data[0] + 
				m_transform.data[9] *
				m_inverseInertiaTensor.data[3] +
				m_transform.data[10] *
				m_inverseInertiaTensor.data[6];
		
		float t57 = m_transform.data[8] * 
				m_inverseInertiaTensor.data[1] + 
				m_transform.data[9] *
				m_inverseInertiaTensor.data[4] +
				m_transform.data[10] *
				m_inverseInertiaTensor.data[7];
		
		float t62 = m_transform.data[8] * 
				m_inverseInertiaTensor.data[2] + 
				m_transform.data[9] *
				m_inverseInertiaTensor.data[5] +
				m_transform.data[10] *
				m_inverseInertiaTensor.data[8];
		
		m_inverseInertiaTensorWorld.data[0] = 
						t4*m_transform.data[0]+
				        t9*m_transform.data[1]+
				        t14*m_transform.data[2];
		m_inverseInertiaTensorWorld.data[1] = 
	    				t4*m_transform.data[4]+
				        t9*m_transform.data[5]+
				        t14*m_transform.data[6];
		m_inverseInertiaTensorWorld.data[2] = 
	    				t4*m_transform.data[8]+
				        t9*m_transform.data[9]+
				        t14*m_transform.data[10];
		m_inverseInertiaTensorWorld.data[3] = 
	    				t28*m_transform.data[0]+
				        t33*m_transform.data[1]+
				        t38*m_transform.data[2];
		m_inverseInertiaTensorWorld.data[4] = 
	    				t28*m_transform.data[4]+
				        t33*m_transform.data[5]+
				        t38*m_transform.data[6];
		m_inverseInertiaTensorWorld.data[5] = 
	    				t28*m_transform.data[8]+
				        t33*m_transform.data[9]+
				        t38*m_transform.data[10];
		m_inverseInertiaTensorWorld.data[6] = 
	    				t52*m_transform.data[0]+
				        t57*m_transform.data[1]+
				        t62*m_transform.data[2];
		m_inverseInertiaTensorWorld.data[7] = 
	    				t52*m_transform.data[4]+
				        t57*m_transform.data[5]+
				        t62*m_transform.data[6];
		m_inverseInertiaTensorWorld.data[8] = 
	    				t52*m_transform.data[8]+
				        t57*m_transform.data[9]+
				        t62*m_transform.data[10];
	}
	
	/** 
	 * Apply a force to this object in the next
	 * iteration.
	 * @param force
	 */
	public void applyForce(Vector3 force)
	{
		m_forceAccum.addAndSet(force);
		// if the object isn't awake, make it awake
		// since it now needs to move
		m_isAwake = true;
	}
	
	/**
	 * Apply a torque to this object in the next
	 * iteration.
	 * @param torque
	 */
	public void applyTorque(Vector3 torque)
	{
		m_torqueAccum.addAndSet(torque);
		m_isAwake = true;
	}
	
	/**
	 * Clear all accumulators at the end of the iteration
	 */
	public void clearAccumulators()
	{
		m_forceAccum.clear();
		m_torqueAccum.clear();
	}
	
	/**
	 * Integrates this object's newtonian values over
	 * 'dt' amount of time
	 */
	public void integrate(float dt)
	{
		// skip integration if we're asleep
		if (!m_isAwake)
			return;
		
		// skip integration if we cannot move
		if (!hasFiniteMass())
			return;
		
		// calculate linear acceleration from current force
		m_lastFrameAcceleration = new Vector3(m_acceleration);
		m_lastFrameAcceleration.addScaledVector(m_forceAccum,  m_inverseMass);
		
		// calculate angular acceleration from torque
		Vector3 angularAcceleration = m_inverseInertiaTensorWorld.transform(m_torqueAccum);
		
		// adjust velocities
		m_velocity.addScaledVector(m_lastFrameAcceleration,  dt);
		m_rotation.addScaledVector(angularAcceleration,  dt);
		
		// impose drag
		m_velocity.multiplyAndSet((float)Math.pow(m_linearDamping,  dt));
		m_rotation.multiplyAndSet((float)Math.pow(m_angularDamping, dt));
		
		// adjust positions
		m_position.addScaledVector(m_velocity,  dt);
		m_orientation.addScaledVector(m_rotation, dt);
		
		// determine the new transformation matrix
		calculateDerivedData();
		
		// clear accumulators
		clearAccumulators();
		
		// check if we need to go to sleep
		if (m_canSleep)
		{
			// use a dot product to quickly determine the 
			// magnitude of these values
			float currentMotion = m_velocity.dotProduct(m_velocity) + m_rotation.dotProduct(m_rotation);
			float bias = (float)Math.pow(g_MotionBias,  dt);
			m_motion = bias*m_motion + (1-bias) * currentMotion;
			
			if (m_motion < g_sleepEpsilon)
				setAwake(false);
			else if (m_motion > 10 * g_sleepEpsilon)
				m_motion = 10 * g_sleepEpsilon;
		}
	}
	
	/**
	 * Zeroes all values in the Z-axis to prevent Z-axis motion
	 * (i.e. to prevent our objects from rotating and translating
	 * outside of a 2D plane)
	 */
	public void zeroZAxis()
	{
		if (m_activated)
		{
			m_orientation.i = 0;
			m_orientation.j = 0;
			m_position.z = 0;
		}
	}

	/**
	 * Stops the RigidBody in place, regardless of forces
	 */
	public void stop()
	{
		m_velocity.clear();
		m_lastFrameAcceleration.clear();
		clearAccumulators();
		m_motion = 0.0f;
		setAwake(false);
	}

	/**
	 * Apply a force at a given point, both given in world space,
	 * this can turn into a force or a torque, since it doesn't
	 * necessarily apply to the center of mass
	 */
	public void applyForceAtPoint(Vector3 force, Vector3 point)
	{
		Vector3 pt = new Vector3(point.x, point.y, point.z);
		pt.subtractAndSet(m_position);
		
		m_forceAccum.addAndSet(force);
		m_torqueAccum.addAndSet(pt.crossProduct(force));
		
		m_isAwake = true;
	}
	
	/**
	 * apply a force at a given point on the body. Takes the closest
	 * point on the body, and applies ApplyForceAtPoint to it.
	 */
	public void applyForceAtBodyPoint(Vector3 force, Vector3 point)
	{
		Vector3 pt = getPointInWorldSpace(point);
		applyForceAtPoint(force, pt);
	}
	
	/**
	 * Returns this point in world coordinates, relative
	 * too the center of mass
	 */
	public Vector3 getPointInWorldSpace(Vector3 point)
	{
		return m_transform.transform(point);
	}
	
	/** 
	 * returns this point in local coordinates
	 */
	public Vector3 getPointInLocalSpace(Vector3 point)
	{
		return m_transform.transformInverse(point);
	}
	
	/**
	 * Gets a direction in world space
	 */
	public Vector3 getDirectionInWorldSpace(Vector3 direction)
	{
		return m_transform.transformDirection(direction);
	}
	
	/**
	 * Gets a direction in local space
	 */
	public Vector3 getDirectionInLocalSpace(Vector3 direction)
	{
		return m_transform.transformInverseDirection(direction);
	}
	
	////////////////////////////////////////////
	// Mass functions
	////////////////////////////////////////////
	/**
	 * Sets the RigidBody's mass directly
	 */
	public void setMass(float mass)
	{
		// throw an error if there's an incorrect mass value.
		// otherwise we're about to cause a divide-by-zero
		// error.
		assert(mass != 0);
		m_mass = mass;
		m_inverseMass = 1 / mass;
	}
	
	/**
	 * Gets the RigidBody's mass
	 */
	public float getMass()
	{
		if (m_inverseMass == 0.0f)
			return Float.MAX_VALUE;
		else
			return m_mass;
	}
	
	/** 
	 * Sets the RigidBody's mass via an inverse mass (much easier to
	 * set an infinite mass object via a zero inverse mass.
	 */
	public void setInverseMass(float inverseMass)
	{
		m_inverseMass = inverseMass;
		if (m_inverseMass == 0.0f)
			m_mass = Float.MAX_VALUE;
		else
			m_mass = 1 / m_inverseMass;
	}
	
	public float getInverseMass()
	{
		return m_inverseMass;
	}
	
	public boolean hasFiniteMass()
	{
		return m_inverseMass > 0.0f;
	}
	
	
	/////////////////////////////////////////
	// Inertia Tensor functions
	/////////////////////////////////////////
	/**
	 * Sets the inertia tensor
	 */
	public void setInertiaTensor(Matrix3 inertiaTensor)
	{
	    m_inverseInertiaTensor.setInverse(inertiaTensor);
	}
	
	/**
	 * Returns the RigidBody's inertia tensor
	 * @return
	 */
	public Matrix3 getInertiaTensor()
	{
		return new Matrix3(m_inverseInertiaTensor);
	}

	// returns the InertiaTensorWorld
	public Matrix3 getInertiaTensorWorld()
	{
		return new Matrix3(m_inverseInertiaTensorWorld);
	}

	/**
	 * Sets the InverseInertiaTensor
	 */
	public void setInverseInertiaTensor(Matrix3 inverseInertiaTensor)
	{
	    m_inverseInertiaTensor = inverseInertiaTensor;
	}

	/**
	 * Returns the inverseInertiaTensor matrix;
	 */
	public Matrix3 getInverseInertiaTensor()
	{
	    return new Matrix3(m_inverseInertiaTensor);
	}

	/**
	 * Returns the inverseInertiaTensorWorld
	 */
	public Matrix3 getInverseInertiaTensorWorld()
	{
	    return new Matrix3(m_inverseInertiaTensorWorld);
	}
	
	//////////////////////////////////////
	// Damping member functions
	//////////////////////////////////////
	public void setDamping(float linearDamping, float angularDamping)
	{
		m_linearDamping = linearDamping;
		m_angularDamping = angularDamping;
	}
	
	public void setLinearDamping(float linearDamping)
	{
		m_linearDamping = linearDamping;
	}
	
	public float getLinearDamping()
	{
		return m_linearDamping;
	}
	
	public void setAngularDamping(float angularDamping)
	{
		m_angularDamping = angularDamping;
	}
	
	public float getAngularDamping()
	{
		 return m_angularDamping;
	}
	
	//////////////////////////////////////
	// Position member functions
	//////////////////////////////////////
	public void setPosition(Vector3 position)
	{
	    m_position = position;
	}

	public void setPosition(float x, float y, float z)
	{
	    m_position.x = x;
	    m_position.y = y;
	    m_position.z = z;
	}

	public Vector3 getPosition()
	{
	    return new Vector3(m_position);
	}
	
	/////////////////////////////////////////
	// Orientation member functions
	/////////////////////////////////////////
	public void setOrientation(Quaternion orientation)
	{
	    m_orientation = orientation;
	    m_orientation.normalize();
	}

	public void setOrientation(float r, float i, float j, float k)
	{
	    m_orientation.r = r;
	    m_orientation.i = i;
	    m_orientation.j = j;
	    m_orientation.k = k;
	    m_orientation.normalize();
	}

	public Quaternion getOrientation()
	{
	    return new Quaternion(m_orientation);
	}

	
	////////////////////////////////
	// Transform member functions
	////////////////////////////////
	public Matrix4 getTransform()
	{
		return new Matrix4(m_transform);
	}

	public void getGLTransform(float matrix[])
	{
		if (matrix.length == 16)
		{
		    matrix[0] = m_transform.data[0];
		    matrix[1] = m_transform.data[4];
		    matrix[2] = m_transform.data[8];
		    matrix[3] = 0;
	
		    matrix[4] = m_transform.data[1];
		    matrix[5] = m_transform.data[5];
		    matrix[6] = m_transform.data[9];
		    matrix[7] = 0;
	
		    matrix[8] = m_transform.data[2];
		    matrix[9] = m_transform.data[6];
		    matrix[10] = m_transform.data[10];
		    matrix[11] = 0;
	
		    matrix[12] = m_transform.data[3];
		    matrix[13] = m_transform.data[7];
		    matrix[14] = m_transform.data[11];
		    matrix[15] = 1;
		}
		else
		{
			System.out.println("Error: Passed a matrix into RigidBody::getGLTransform() that isn't of size 16!");
		}
	}

	/* not used in this Java Port
	public D3DXMATRIX RigidBody::getDXTransform()
	{
		// convert the object's transformation matrix into
		// D3DXMATRIX format and return it.

		// Need to explicitly set the last 4 components to 
		// 0, 0, 0, 1 since we assume that information (we
		// don't waste 4 integers storing it)
		D3DXMATRIX w;

		w = transformMatrix.ConvertToDX();

		return w;
	}*/
	
	public float[] getGLMatrix()
	{
		float[] retval = new float[16];
		
		retval[0] = m_transform.data[0];
		retval[1] = m_transform.data[4];
		retval[2] = m_transform.data[8];
		retval[3] = 0;
		
		retval[4] = m_transform.data[1];
		retval[5] = m_transform.data[5];
		retval[6] = m_transform.data[9];
		retval[7] = 0;
		
		retval[8] = m_transform.data[2];
		retval[9] = m_transform.data[6];
		retval[10] = m_transform.data[10];
		retval[11] = 0;
		
		retval[12] = m_transform.data[3];
		retval[13] = m_transform.data[7];
		retval[14] = m_transform.data[11];
		retval[15] = 1;
		
		return retval;
	}

	
	
	//////////////////////////////////////
	// Points and Directions
	//////////////////////////////////////
	
	
	/////////////////////////////////////
	// Velocity member functions
	/////////////////////////////////////
	public void setVelocity(Vector3 velocity)
	{
	    m_velocity = velocity;
	}

	public void setVelocity(float x, float y, float z)
	{
	    m_velocity.x = x;
	    m_velocity.y = y;
	    m_velocity.z = z;
	}

	public Vector3 getVelocity()
	{
	    return new Vector3(m_velocity);
	}

	public void addVelocity(Vector3 deltaVelocity)
	{
	    m_velocity.addAndSet(deltaVelocity);
	}
	
	/////////////////////////////////////////
	// Rotation member functions
	/////////////////////////////////////////
	public void setRotation(Vector3 rotation)
	{
	    m_rotation = rotation;
	}

	public void setRotation(float x, float y, float z)
	{
		m_rotation.x = x;
		m_rotation.y = y;
		m_rotation.z = z;
	}

	public Vector3 getRotation()
	{
	    return new Vector3(m_rotation);
	}

	public void addRotation(Vector3 deltaRotation)
	{
	    m_rotation.addAndSet(deltaRotation);
	}
	
	///////////////////////////////////////////
	// Acceleration member functions
	///////////////////////////////////////////
	public void setAcceleration(Vector3 acceleration)
	{
	    m_acceleration = acceleration;
	}

	public void setAcceleration(float x, float y, float z)
	{
		m_acceleration.x = x;
		m_acceleration.y = y;
		m_acceleration.z = z;
	}

	public Vector3 getAcceleration()
	{
	    return new Vector3(m_acceleration);
	}
	
	////////////////////////////////////////
	// State-handling functions
	////////////////////////////////////////
	public void setAwake(boolean awake)
	{
	    if (awake) {
	        m_isAwake= true;

	        // Add a bit of motion to avoid it falling asleep immediately.
	        m_motion = g_sleepEpsilon*2.0f;
	    } else {
			m_motion = 0.0f;
	        m_isAwake = false;
	        m_velocity.clear();
	        m_rotation.clear();
	    }
	}

	public void setCanSleep(boolean canSleep)
	{
	    m_canSleep = canSleep;

	    if (!m_canSleep && !m_isAwake) setAwake(true);
	}

	public boolean getAwake()
	{
		return m_isAwake;
	}

	public boolean getCanSleep()
	{
		return m_canSleep;
	}
	
	public boolean getActivated()
	{
		return m_activated;
	}
	
	public void deactivate()
	{
		m_activated = false;
	}
	
	public void activate()
	{
		m_activated = true;
	}
	
	/////////////////////////////
	// Misc
	/////////////////////////////
	public Vector3 getLastFrameAcceleration()
	{
	    return new Vector3(m_lastFrameAcceleration);
	}
	
	public void setMaterial(ObjectMaterial material)
	{
		m_material = material;
	}
	
	public ObjectMaterial getMaterial()
	{
		return m_material;
	}
}
